/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : app_freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "tim.h"
#include "dac.h"
#include "arm_math.h"
#include "usbd_cdc_if.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
typedef struct 
{
	double la;
	float K1_8;
	float K2_8;
    float K3_8;         /* Added @2024.06.28 */
	
	float alpha_a;
	float beta_a;	
    float gamma_a;      /* Added @2024.06.28 */
}KAL_Config_Integer_ParaType;

KAL_Config_Integer_ParaType KAL_Para_Integer;
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
int32_t alpha_a_Integer = 65536;
int32_t beta_a_Integer = 0;
int32_t gamma_a_Integer = 0;
extern uint8_t UsbSend[10];
extern USBD_HandleTypeDef hUsbDeviceFS;
/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .priority = (osPriority_t) osPriorityLow,
  .stack_size = 128 * 4
};
/* Definitions for PWMtask */
osThreadId_t PWMtaskHandle;
const osThreadAttr_t PWMtask_attributes = {
  .name = "PWMtask",
  .priority = (osPriority_t) osPriorityNormal4,
  .stack_size = 128 * 4
};
/* Definitions for DACtask */
osThreadId_t DACtaskHandle;
const osThreadAttr_t DACtask_attributes = {
  .name = "DACtask",
  .priority = (osPriority_t) osPriorityLow6,
  .stack_size = 128 * 4
};
/* Definitions for DataProcess */
osThreadId_t DataProcessHandle;
const osThreadAttr_t DataProcess_attributes = {
  .name = "DataProcess",
  .priority = (osPriority_t) osPriorityRealtime7,
  .stack_size = 128 * 4
};
/* Definitions for Message */
osThreadId_t MessageHandle;
const osThreadAttr_t Message_attributes = {
  .name = "Message",
  .priority = (osPriority_t) osPriorityRealtime1,
  .stack_size = 128 * 4
};
/* Definitions for USBReceiveQueue */
osMessageQueueId_t USBReceiveQueueHandle;
const osMessageQueueAttr_t USBReceiveQueue_attributes = {
  .name = "USBReceiveQueue"
};
/* Definitions for DacQueue */
osMessageQueueId_t DacQueueHandle;
const osMessageQueueAttr_t DacQueue_attributes = {
  .name = "DacQueue"
};
/* Definitions for PWMQueue */
osMessageQueueId_t PWMQueueHandle;
const osMessageQueueAttr_t PWMQueue_attributes = {
  .name = "PWMQueue"
};
/* Definitions for ADCQueue */
osMessageQueueId_t ADCQueueHandle;
const osMessageQueueAttr_t ADCQueue_attributes = {
  .name = "ADCQueue"
};
/* Definitions for ADCcomplete */
osSemaphoreId_t ADCcompleteHandle;
const osSemaphoreAttr_t ADCcomplete_attributes = {
  .name = "ADCcomplete"
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
uint32_t kal_filter_Integer(KAL_Config_Integer_ParaType* cfg, uint32_t xin);
/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);
void PwmTask(void *argument);
void DacTask(void *argument);
void ADCData(void *argument);
void Communacation(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* Create the semaphores(s) */
  /* creation of ADCcomplete */
  ADCcompleteHandle = osSemaphoreNew(1, 0, &ADCcomplete_attributes);

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the queue(s) */
  /* creation of USBReceiveQueue */
  USBReceiveQueueHandle = osMessageQueueNew (40, sizeof(uint8_t), &USBReceiveQueue_attributes);

  /* creation of DacQueue */
  DacQueueHandle = osMessageQueueNew (5, sizeof(int32_t), &DacQueue_attributes);

  /* creation of PWMQueue */
  PWMQueueHandle = osMessageQueueNew (5, sizeof(int32_t), &PWMQueue_attributes);

  /* creation of ADCQueue */
  ADCQueueHandle = osMessageQueueNew (5, sizeof(int32_t), &ADCQueue_attributes);

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* creation of PWMtask */
  PWMtaskHandle = osThreadNew(PwmTask, NULL, &PWMtask_attributes);

  /* creation of DACtask */
  DACtaskHandle = osThreadNew(DacTask, NULL, &DACtask_attributes);

  /* creation of DataProcess */
  DataProcessHandle = osThreadNew(ADCData, NULL, &DataProcess_attributes);

  /* creation of Message */
  MessageHandle = osThreadNew(Communacation, NULL, &Message_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
	uint8_t i = 0;
  /* Infinite loop */
  for(;;)
  {
		osSemaphoreRelease(ADCcompleteHandle);
    osDelay(1000);
  }
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_PwmTask */
/**
* @brief Function implementing the PWMtask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_PwmTask */
void PwmTask(void *argument)
{
  /* USER CODE BEGIN PwmTask */
  /* Infinite loop */
	uint8_t QueueStatus = 0;
	uint16_t Tim3SetValue = 0;
	int32_t SensorAngleFiltered = 0;
  for(;;)
  {
		
			QueueStatus = osMessageQueueGet(PWMQueueHandle,&SensorAngleFiltered,NULL,osWaitForever);
			if(	QueueStatus == osOK)
			{
				TIM_OC_InitTypeDef	sConfigOC;
				sConfigOC.OCMode = TIM_OCMODE_COMBINED_PWM1;
				Tim3SetValue=SensorAngleFiltered*9999/131071;
				sConfigOC.Pulse = Tim3SetValue;
				sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
				sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
				if (HAL_TIM_OC_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
				{
					Error_Handler();
				}
			}
			osDelay(1);
  }
  /* USER CODE END PwmTask */
}

/* USER CODE BEGIN Header_DacTask */
/**
* @brief Function implementing the DACtask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_DacTask */
void DacTask(void *argument)
{
  /* USER CODE BEGIN DacTask */
	uint8_t QueueStatus = 0;
	uint16_t Set_Value = 0;
	int32_t SensorAngleFiltered = 0;
  /* Infinite loop */
  for(;;)
  {
		QueueStatus = osMessageQueueGet(DacQueueHandle,&SensorAngleFiltered,NULL,osWaitForever);
		if(	QueueStatus == osOK)
		{		
			Set_Value = SensorAngleFiltered*4095/131071;				
			if (HAL_DAC_SetValue(&hdac1,DAC_CHANNEL_2,DAC_ALIGN_12B_R,Set_Value) != HAL_OK) {
					Error_Handler();
			}
		}
			osDelay(5);
  }
  /* USER CODE END DacTask */
}

/* USER CODE BEGIN Header_ADCData */
/**
* @brief Function implementing the DataProcess thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_ADCData */
void ADCData(void *argument)
{
  /* USER CODE BEGIN ADCData */
	portFLOAT SensorAngle = 0.0;
	uint8_t putstate = 0;
	int16_t RawData[2] = {0,0};
	float Angle,Anglebuff,cordic_outf,CordicIn[2];
	uint32_t DMA_adc2mem[4] ={0,0};
	int32_t SensorAngleFiltered = 0;
  /* Infinite loop */
  for(;;)
  {
			osSemaphoreAcquire(ADCcompleteHandle,osWaitForever);
			RawData[0] = 2048 - *((int16_t*)&DMA_adc2mem+1);   //msin
	    RawData[1] = *(int16_t*)&DMA_adc2mem - 2048;       //mcos
			
			CordicIn[0] = (float)RawData[0];
			CordicIn[1] = (float)RawData[1];
			
			cordic_outf = atan2f(CordicIn[1],CordicIn[0]); 
			Angle =	cordic_outf*57.295779513f;
			
			Anglebuff = Angle < 0 ? Angle + 360: Angle; 
			
			SensorAngle = Anglebuff* 364.088889f;
			SensorAngle = SensorAngle < 0 ? SensorAngle + 131071 : SensorAngle;			
			SensorAngleFiltered =kal_filter_Integer(&KAL_Para_Integer,SensorAngle);	
			putstate = osMessageQueuePut(PWMQueueHandle,&SensorAngleFiltered,1,1);
			putstate = osMessageQueuePut(DacQueueHandle,&SensorAngleFiltered,1,1);
			osDelay(1);
  }
  /* USER CODE END ADCData */
}

/* USER CODE BEGIN Header_Communacation */
/**
* @brief Function implementing the Message thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Communacation */
void Communacation(void *argument)
{
  /* USER CODE BEGIN Communacation */
	uint8_t RX_buffer[20];
	uint8_t state = 0;
	float SensorAngleFiltered = 0;
  /* Infinite loop */
  for(;;)
  {
		state = osMessageQueueGet(USBReceiveQueueHandle,RX_buffer,NULL,osWaitForever);
		if(state == osOK)
		{
				if((RX_buffer[0]==0x5a)&&(RX_buffer[1]==0x55))
				{
					USBD_CDC_SetTxBuffer(&hUsbDeviceFS, (uint8_t*)&SensorAngleFiltered, 4);
					USBD_CDC_TransmitPacket(&hUsbDeviceFS);
				}
		}
    osDelay(1);
  }
  /* USER CODE END Communacation */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
uint32_t kal_filter_Integer(KAL_Config_Integer_ParaType* cfg, uint32_t xin)
{
    int32_t    xi;

    int32_t da = 0;

    int64_t tmp0,tmp1,tmp2,tmp3;

    da = xin - alpha_a_Integer;

    /* Numerical continuity */
    if (da <= -65536)
    {
        xi = xin + 131072;
    }
    else if (da >= 65536)
    {
        xi = xin - 131072;
    }
    else
    {
        xi = xin;
    }

	/*
    kal_param[0][0] = 968567;
    kal_param[0][1] = 24;
    kal_param[0][2] = 0;
    kal_param[0][3] = 80008;

    kal_param[1][0] = -126988751;
    kal_param[1][1] = 1045401;
    kal_param[1][2] = 26;
    kal_param[1][3] = 126988750;

    kal_param[2][0] = -2519446;
    kal_param[2][1] = -63;
    kal_param[2][2] = 1048575;
    kal_param[2][3] = 2519445;
    */
	
	/*
    kal_param[0][0] = 1022483;
    kal_param[0][1] = 63;
    kal_param[0][2] = 0;
    kal_param[0][3] = 26092;

    kal_param[1][0] = -5259816;
    kal_param[1][1] = 1048247;
    kal_param[1][2] = 65;
    kal_param[1][3] = 5259815;

    kal_param[2][0] = -33135;
    kal_param[2][1] = -3;
    kal_param[2][2] = 1048575;
    kal_param[2][3] = 33134;
    */
	
    tmp0 = alpha_a_Integer;
    tmp0 = tmp0 * 1022483;  // tmp0 = tmp0 * kal_param[0][0];

    tmp1 = beta_a_Integer;
    tmp1 = tmp1 * 63;  //tmp1 = tmp1 * kal_param[0][1];

    tmp2 = gamma_a_Integer;
    tmp2 = tmp2 * 0;  //tmp2 = tmp2 * kal_param[0][2];

    tmp3 = xi;
    tmp3 = tmp3 * 26092;  //tmp3 = tmp3 * kal_param[0][3];

    tmp3 = tmp3 + tmp2 + tmp1 + tmp0;

    cfg->alpha_a = tmp3>>20;  
    ////////////////////////////////////////

    tmp0 = alpha_a_Integer;
    tmp0 = tmp0 * -5259816;     //tmp0 = tmp0 * kal_param[1][0];

    tmp1 = beta_a_Integer;
    tmp1 = tmp1 * 1048247;     //tmp1 = tmp1 * kal_param[1][1];

    tmp2 = gamma_a_Integer;
    tmp2 = tmp2 * 65;     //tmp2 = tmp2 * kal_param[1][2];

    tmp3 = xi;
    tmp3 = tmp3 * 5259815;     //tmp3 = tmp3 * kal_param[1][3];

    tmp3 = tmp3 + tmp2 + tmp1 + tmp0;

    cfg->beta_a = tmp3 >> 20;
    //////////////////////////////////////////////
    tmp0 = alpha_a_Integer;
    tmp0 = tmp0 * -33135;   //tmp0 = tmp0 * kal_param[2][0];

    tmp1 = beta_a_Integer;
    tmp1 = tmp1 * -3;   //tmp1 = tmp1 * kal_param[2][1];

    tmp2 = gamma_a_Integer;
    tmp2 = tmp2 * 1048575;   //tmp2 = tmp2 * kal_param[2][2];

    tmp3 = xi;
    tmp3 = tmp3 * 33134;   //tmp3 = tmp3 * kal_param[2][3];

    tmp3 = tmp3 + tmp2 + tmp1 + tmp0;

    cfg->gamma_a = tmp3 >> 20;

    //////////////////////////////////////////////

    /* the angle must be [0, 2PI) */
    if (cfg->alpha_a < 0)
    {
        cfg->alpha_a += 131072;
    }
    else if (cfg->alpha_a >= 131072)
    {
        cfg->alpha_a -= 131072;
    }

    alpha_a_Integer = cfg->alpha_a;
    beta_a_Integer = cfg->beta_a;
    gamma_a_Integer = cfg->gamma_a;

    return cfg->alpha_a;
}
/* USER CODE END Application */

